#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <mqueue.h>
#include <pthread.h>
#include <errno.h>
#include <signal.h>
#include <sys/mman.h>
#include "ILSimulator.h"
#include "Common_Types.h"

mqd_t mqList[5];
char mqIdList[5][20];
BOOL quit = FALSE;    // signal flag
int main(int argc, char **argv) {
	if (mlockall(MCL_CURRENT | MCL_FUTURE)) {/*Lock all process memory pages in RAM(disable paging)*/
	   		perror("mlockall failed:");
	   		exit(EXIT_FAILURE);
	}
	struct sched_param scheduling_parameters;
	scheduling_parameters.sched_priority = sched_get_priority_max(SCHED_FIFO);
	sched_setscheduler(0, SCHED_FIFO, &scheduling_parameters);

	NOTIFY("IL Simulator Starting\n");
	U32 i;
	int rc;
	pthread_t thread1;
	char arg[] = {0x01, 0x02};
	void* threadArg = arg;

	openMQ();

	rc = pthread_create(&thread1, NULL, &ListenMQ, threadArg);
	int c;
	while ((c = getopt(argc, argv, "du123:")) != -1) {
		switch (c)
		{
			case 'd':
				testDART();
				break;
			case 'u':
				testURT();
				break;
			case '?':
				printf("not valid option\n");
				break;
		}
	}

	pthread_join(thread1, NULL);

	for(i = 0; i<MQUEUE_COUNT; i++ ) {
		mq_close( mqList[i]);
	}
	NOTIFY("terminating Simulator\n");
	munlockall(); /*unlock process memory pages in RAM*/
	exit(EXIT_SUCCESS);

}
void openMQ()
{
	U32 i;
    struct mq_attr wanted_attrs;
    wanted_attrs.mq_flags =   0;
    wanted_attrs.mq_maxmsg =  10;
    wanted_attrs.mq_msgsize = MQUEUE_MAX_MSG_SIZE;
    wanted_attrs.mq_curmsgs = 0;

	strcpy(mqIdList[0], "/MQUEUE_CL2AP_ID");
	strcpy(mqIdList[1], "/MQUEUE_CL2IL_ID");
	strcpy(mqIdList[2], "/MQUEUE_AP2CL_ID");
	strcpy(mqIdList[3], "/MQUEUE_IL2CLREQ_ID");
	strcpy(mqIdList[4], "/MQUEUE_IL2CLRT_ID");


	mqList[MQUEUE_CL2AP_ind] = mq_open(mqIdList[MQUEUE_CL2AP_ind], O_RDWR | O_CREAT | O_NONBLOCK, S_IRWXU | S_IRWXG , &wanted_attrs);
	mqList[MQUEUE_CL2IL_ind] = mq_open(mqIdList[MQUEUE_CL2IL_ind], O_RDWR | O_CREAT | O_NONBLOCK, S_IRWXU | S_IRWXG , &wanted_attrs);

	mqList[MQUEUE_AP2CL_ind]    = mq_open(mqIdList[MQUEUE_AP2CL_ind],    O_RDWR | O_CREAT | O_NONBLOCK, S_IRWXU | S_IRWXG , &wanted_attrs);
	mqList[MQUEUE_IL2CLREQ_ind] = mq_open(mqIdList[MQUEUE_IL2CLREQ_ind], O_RDWR | O_CREAT | O_NONBLOCK, S_IRWXU | S_IRWXG , &wanted_attrs);
	mqList[MQUEUE_IL2CLRT_ind]  = mq_open(mqIdList[MQUEUE_IL2CLRT_ind],  O_RDWR | O_CREAT | O_NONBLOCK, S_IRWXU | S_IRWXG , &wanted_attrs);


	for(i = 0; i<MQUEUE_COUNT; i++ ) {
		if( mqList[i] == -1 ) {
			PERROR("Cannot Open Message Queue[%d]\n", i);
			exit(EXIT_FAILURE);
		}
	}

}
void got_signal(int arg)
{
    quit = TRUE;
}
void* ListenMQ(void* threadArg)
{
	TimeInternal timeout = { 1, 0 };

    int ret;
    int length;
    char msg[MQUEUE_MAX_MSG_SIZE];
	struct sigaction sa;
	memset( &sa, 0, sizeof(sa) );
	sa.sa_handler = got_signal;
	sigfillset(&sa.sa_mask);
	sigaction(SIGINT,&sa,NULL);
	sigaction(SIGTERM,&sa,NULL);
	sigaction(SIGTSTP,&sa, NULL);

	for( ;;) {

		ret = selectMsgQ(&timeout);

		if(ret < 0) {
			PERROR("failed to poll sockets");
			//return;
		} else if(!ret) {
			//return;
		}
		else {
			DBG("ret:%d\n", ret);
			length = mq_receive(mqList[MQUEUE_CL2IL_ind], msg, MQUEUE_MAX_MSG_SIZE, 0);
			if(length > 0)
			{
				DBG("Msg From IL received: <%d>, %d\n", msg[0], length);
			}
			else {
				length = mq_receive(mqList[MQUEUE_CL2AP_ind], msg, MQUEUE_MAX_MSG_SIZE, 0);
				if(length > 0)
				{
					DBG("Msg From AP received: <%c>, %d\n", msg[length-1], length);
				}
			}
		}

		if( quit )
			  break;
	}
	pthread_exit(NULL);
	return NULL;
}


U8 selectMsgQ(TimeInternal* timeout)
{
	U32 i;
	int ret, nfds = 0;
	fd_set readfds;
	struct timeval tv, *tv_ptr;

	if (timeout < 0)
		return FALSE;

	FD_ZERO(&readfds);
	for(i=0; i<2; i++) {
		if(mqList[i] > nfds)
			nfds = mqList[i];
		FD_SET(mqList[i], &readfds);
	}

	if (timeout) {
		tv.tv_sec = timeout->seconds;
		tv.tv_usec = timeout->nanoseconds / 1000;
		tv_ptr = &tv;
	} else
		tv_ptr = 0;

	ret = select(nfds + 1, &readfds, 0, 0, tv_ptr) > 0;

	if (ret < 0) {
		if (errno == EAGAIN || errno == EINTR)
			return 0;
	}
	return ret;
}

void testDART()
{
	TimeInternal tWait = {0, 1000000};
	U32 i = 1;
	DBG("test Dart");
	REQRT(i);
	nanoSleep(&tWait);
	testIL2CLRT_DART();
	for(i = 2; i < 1000000; i++) {
		nanoSleep(&tWait);
		if( REQRT(i) < 0 )
			exit(EXIT_FAILURE);
		if(quit)
			exit(EXIT_SUCCESS);
	}

}

void testIL2CLRT_DART()
{
	U32 length;
	char msg[MQUEUE_MAX_MSG_SIZE];
	U32 i, j;

	U32 CLHeaderLength;

	U8 payload[350];
	U16 payloadSize = 257;

	DART_MESSAGE_PARAMETER_TYPE dartPar;
	dartPar.numberOfFree = 1;
	dartPar.numberOfNew = 2;

	dartPar.Free[0].DeviceChannel.nodeId = 1;
	dartPar.Free[0].DeviceChannel.ch = 2;

	dartPar.Free[1].DeviceChannel.nodeId = 7;
	dartPar.Free[1].DeviceChannel.ch = 1;

	dartPar.New[0].requiredSlots = 1;
	dartPar.New[0].DeviceChannel.nodeId = 1;
	dartPar.New[0].DeviceChannel.ch = 1;

	dartPar.New[1].requiredSlots = 2;
	dartPar.New[1].DeviceChannel.nodeId = 1;
	dartPar.New[1].DeviceChannel.ch = 2;

	dartPar.New[2].requiredSlots = 2;
	dartPar.New[2].DeviceChannel.nodeId = 2;
	dartPar.New[2].DeviceChannel.ch = 1;

	DART_MESSAGE_PARAMETER_TYPE* parPtr = (DART_MESSAGE_PARAMETER_TYPE* )(&dartPar);


	/*copy message parameters in to the buffer that will be sent to IL  */
	msg[0] = parPtr->numberOfNew;
	msg[1] = parPtr->numberOfFree;

	for( i = 0; i < parPtr->numberOfNew; i++) {
		memcpy( &msg[ 2 + i*sizeof(NEW_SLOTS_TYPE) ], &(parPtr->New[i]), sizeof(NEW_SLOTS_TYPE));
	}
	for( j = 0; j < parPtr->numberOfFree; j++ ) {
		memcpy( &msg[ 2 + i*sizeof(NEW_SLOTS_TYPE) + j*sizeof(FREE_SLOTS_TYPE) ], &(parPtr->Free[j]), sizeof(FREE_SLOTS_TYPE));
	}

	CLHeaderLength = 2 + i*sizeof(NEW_SLOTS_TYPE) + j*sizeof(FREE_SLOTS_TYPE) ;


	/*copy payload size*/
	memcpy( (U16* )&msg[CLHeaderLength], &payloadSize, sizeof(payloadSize)  );
	CLHeaderLength += sizeof(payloadSize);


	/*copy the payload */
	memcpy( &msg[CLHeaderLength], &payload[0], payloadSize  );

	length = mq_send(mqList[MQUEUE_IL2CLRT_ind], msg, CLHeaderLength + payloadSize, 0); // (msqid, msg, strlen(msg), 0);//IPC_NOWAIT);

}

void testURT()
{
	TimeInternal curTime;
	TimeInternal t;
	TimeInternal t1, t2;

    t.seconds = 0;
	/*initial configuration*/
    t.nanoseconds = 1000;
    nanoSleep(&t);

    REQRT(1);

    testIL2CLRT1();

    t.nanoseconds = (2)*1000000;
    getTime(&curTime);
    nanoSleep(&t);
    REQRT(2);
    testIL2CLRT0();
    nanoSleep(&t);
getTime(&t1);
    REQRT(3);getTime(&t2);
    testIL2CLRT2();

    t.nanoseconds = (5)*1000000;
    nanoSleep(&t);
    REQRT(4);
    testIL2CLRT3();
    DBG("time difference= %d", subtractTime( &t2, &t1).nanoseconds );

}

int REQRT(U8 sequenceNumber)
{
	TimeInternal t1;
	char msg[15];
	int length;

	msg[0] = (char)sequenceNumber;

	getTime(&t1);
	*(U32* )&msg[1] =  t1.seconds;
	*(U32* )&msg[5] =  t1.nanoseconds;

	length = mq_send(mqList[MQUEUE_IL2CLREQ_ind], msg, 15, 0);

	DBG("REQRT mqId: %d length:%d\n", mqList[MQUEUE_IL2CLREQ_ind], length);

	return length;

}
void testIL2CLRT0()
{
	U32 length;
	char msg[MQUEUE_MAX_MSG_SIZE];

	U32 i;
	U32 CLHeaderLength;
	U8 payload[] = "1234567890";
	U16 payloadSize = 10;

    URT_MESSAGE_PARAMETER_TYPE urtPar;
    urtPar.totalNumberOfRequests = 0;

    urtPar.comRequests[0].nodeId = 2;
    urtPar.comRequests[0].ch = 2;
    urtPar.comRequests[0].eT = 2;
    urtPar.comRequests[0].dT = 10;

    URT_MESSAGE_PARAMETER_TYPE* parPtr = (URT_MESSAGE_PARAMETER_TYPE* )(&urtPar);

	/*copy message parameters in to the buffer that will be sent to IL  */
    msg[0] = parPtr->totalNumberOfRequests;

	for( i = 0; i < parPtr->totalNumberOfRequests; i++) {
		memcpy( &msg[ 1 + i*sizeof(URT_COMMUNICATION_REQUEST_TYPE) ], &(parPtr->comRequests[i]), sizeof(URT_COMMUNICATION_REQUEST_TYPE));
	}

	CLHeaderLength = 1 + i*sizeof(URT_COMMUNICATION_REQUEST_TYPE);

	/*copy payload size*/
	memcpy( (U16* )&msg[CLHeaderLength], &payloadSize, sizeof(payloadSize)  );
	CLHeaderLength += sizeof(payloadSize);

	/*copy the payload */
	memcpy( &msg[CLHeaderLength], &payload[0], payloadSize  );

	length = mq_send(mqList[MQUEUE_IL2CLRT_ind], msg, CLHeaderLength + payloadSize, 0); // (msqid, msg, strlen(msg), 0);//IPC_NOWAIT);

	//URT_MESSAGE_PARAMETER_TYPE newPar = IL2CLRT(msg, CLHeaderLength + payloadSize);
}

void testIL2CLRT1()
{
	U32 length;
	char msg[MQUEUE_MAX_MSG_SIZE];

	U32 i;
	U32 CLHeaderLength;
	U8 payload[] = "1234567890";
	U16 payloadSize = 10;

    URT_MESSAGE_PARAMETER_TYPE urtPar;
    urtPar.totalNumberOfRequests = 1;

    urtPar.comRequests[0].nodeId = 2;
    urtPar.comRequests[0].ch = 2;
    urtPar.comRequests[0].eT = 2;
    urtPar.comRequests[0].dT = 10;

    URT_MESSAGE_PARAMETER_TYPE* parPtr = (URT_MESSAGE_PARAMETER_TYPE* )(&urtPar);

	/*copy message parameters in to the buffer that will be sent to IL  */
    msg[0] = parPtr->totalNumberOfRequests;

	for( i = 0; i < parPtr->totalNumberOfRequests; i++) {
		memcpy( &msg[ 1 + i*sizeof(URT_COMMUNICATION_REQUEST_TYPE) ], &(parPtr->comRequests[i]), sizeof(URT_COMMUNICATION_REQUEST_TYPE));
	}

	CLHeaderLength = 1 + i*sizeof(URT_COMMUNICATION_REQUEST_TYPE);

	/*copy payload size*/
	memcpy( (U16* )&msg[CLHeaderLength], &payloadSize, sizeof(payloadSize)  );
	CLHeaderLength += sizeof(payloadSize);

	/*copy the payload */
	memcpy( &msg[CLHeaderLength], &payload[0], payloadSize  );
	//printf("length: %d", CLHeaderLength + payloadSize);

	length = mq_send(mqList[MQUEUE_IL2CLRT_ind], msg, CLHeaderLength + payloadSize, 0); // (msqid, msg, strlen(msg), 0);//IPC_NOWAIT);

}

void testIL2CLRT2()
{
	U32 length;
	char msg[MQUEUE_MAX_MSG_SIZE];

	U32 i;
	U32 CLHeaderLength;
	U8 payload[] = "1234567890" ;
	U16 payloadSize = 10;

    URT_MESSAGE_PARAMETER_TYPE urtPar;
    urtPar.totalNumberOfRequests = 3;

    urtPar.comRequests[0].nodeId = 2;
    urtPar.comRequests[0].ch = 1;
    urtPar.comRequests[0].eT = 2;
    urtPar.comRequests[0].dT = 5;

    urtPar.comRequests[1].nodeId = 5;
    urtPar.comRequests[1].ch = 1;
    urtPar.comRequests[1].eT = 2;
    urtPar.comRequests[1].dT = 5;

    urtPar.comRequests[2].nodeId = 2;
    urtPar.comRequests[2].ch = 2;
    urtPar.comRequests[2].eT = 2;
    urtPar.comRequests[2].dT = 10;

    URT_MESSAGE_PARAMETER_TYPE* parPtr = (URT_MESSAGE_PARAMETER_TYPE* )(&urtPar);


	/*copy message parameters in to the buffer that will be sent to IL  */
    msg[0] = parPtr->totalNumberOfRequests;

	for( i = 0; i < parPtr->totalNumberOfRequests; i++) {
		memcpy( &msg[ 1 + i*sizeof(URT_COMMUNICATION_REQUEST_TYPE) ], &(parPtr->comRequests[i]), sizeof(URT_COMMUNICATION_REQUEST_TYPE));
	}

	CLHeaderLength = 1 + i*sizeof(URT_COMMUNICATION_REQUEST_TYPE);

	/*copy payload size*/
	memcpy( (U16* )&msg[CLHeaderLength], &payloadSize, sizeof(payloadSize)  );
	CLHeaderLength += sizeof(payloadSize);

	/*copy the payload */
	memcpy( &msg[CLHeaderLength], &payload[0], payloadSize  );

	length = mq_send(mqList[MQUEUE_IL2CLRT_ind], msg, CLHeaderLength + payloadSize, 0); // (msqid, msg, strlen(msg), 0);//IPC_NOWAIT);

}
void testIL2CLRT3()
{
	U32 length;
	char msg[MQUEUE_MAX_MSG_SIZE];

	U32 i;
	U32 CLHeaderLength;
	U8 payload[] = "1234567890" ;
	U16 payloadSize = 10;

    URT_MESSAGE_PARAMETER_TYPE urtPar;
    urtPar.totalNumberOfRequests = 1;

    urtPar.comRequests[0].nodeId = 2;
    urtPar.comRequests[0].ch = 1;
    urtPar.comRequests[0].eT = 2;
    urtPar.comRequests[0].dT = 5;

    URT_MESSAGE_PARAMETER_TYPE* parPtr = (URT_MESSAGE_PARAMETER_TYPE* )(&urtPar);

	/*copy message parameters in to the buffer that will be sent to IL  */
    msg[0] = parPtr->totalNumberOfRequests;

	for( i = 0; i < parPtr->totalNumberOfRequests; i++) {
		memcpy( &msg[ 1 + i*sizeof(URT_COMMUNICATION_REQUEST_TYPE) ], &(parPtr->comRequests[i]), sizeof(URT_COMMUNICATION_REQUEST_TYPE));
	}

	CLHeaderLength = 1 + i*sizeof(URT_COMMUNICATION_REQUEST_TYPE);

	/*copy payload size*/
	memcpy( (U16* )&msg[CLHeaderLength], &payloadSize, sizeof(payloadSize)  );
	CLHeaderLength += sizeof(payloadSize);

	/*copy the payload */
	memcpy( &msg[CLHeaderLength], &payload[0], payloadSize  );

	length = mq_send(mqList[MQUEUE_IL2CLRT_ind], msg, CLHeaderLength + payloadSize, 0); // (msqid, msg, strlen(msg), 0);//IPC_NOWAIT);

}


